
#include <caml/mlvalues.h>
#include <caml/callback.h>
#include <caml/memory.h>
#include <caml/alloc.h>
#include "cfsqpusr.h"

/* -------------------------------------------------------------------------- */
/*  Test                                                                      */
/* -------------------------------------------------------------------------- */

value incr_arr_(value v_v,value n_v){
  CAMLparam2(v_v,n_v);
  CAMLlocal1(ret_v);
  int n = Int_val(n_v);
  int i = 0;
  double d;
  ret_v = caml_alloc(n * Double_wosize, Double_array_tag);

  for(i=0;i<n;i++){
    d = Double_field(v_v, i);
    Store_double_field(ret_v, i, d + 1.0); 
  }

  CAMLreturn(ret_v);
}

/* -------------------------------------------------------------------------- */
/*  Callbacks                                                                 */
/* -------------------------------------------------------------------------- */

value double_array_to_value(double* x,int n){
  CAMLparam0();
  CAMLlocal1(res);
  res = caml_alloc(n * Double_wosize, Double_array_tag);
  int i = 0;
  for(i=0;i<n;i++){
    Store_double_field (res, i, x[i]);
  }
  CAMLreturn(res);
}

/* call the objective function (see the cfsqp manual, p. 27) */
void caml_obj_fun(int nparam,    // (in) dimension of x
                  int j,         // (in) number of the obj function to be computed.  We always have a sinle obj fun, so this is fixed.
                  double* x,     // (in) evaluate at this point
                  double* ret,   // (out) pointer to the value of the jth obj fun at x
                  void* dummy)   // (inout) not used (by me)
{
  CAMLparam0();
  CAMLlocal3(iter,f,res);
  f = *(caml_named_value("caml_obj_fun"));
  iter = double_array_to_value(x,nparam);
  res = caml_callback(f,iter);
  ret[0] = Double_val(res);
  CAMLreturn0;
}

/* call the constraint functions */
void caml_constr_fun(int nparam,   // (in) dimension of x
                     int j,        // (in) jth constraint
                     double* x,    // (in) eval at this point
                     double* ret,  // (out) val of jth constraint
                     void* dummy){ // not used
  CAMLparam0();
  CAMLlocal4(iter,f,res,j_v);
  j_v = Val_int(j);
  f = *(caml_named_value("caml_constr_fun"));
  iter = double_array_to_value(x,nparam);
  res = caml_callback2(f,j_v,iter);
  ret[0] = Double_val(res);
  CAMLreturn0;
}

value cfsqp_minimize_(value numargs_v,
                     value nconstr_v,
                     value xmin_v,
                     value xmax_v,
                     value init_v){
  CAMLparam5(numargs_v,nconstr_v,xmin_v,xmax_v,init_v);
  CAMLlocal1(opt_v);

  /* There are a bunch of arguments to the primitive csfqp function, ~30.
     We only use a few. We keep the names from the manual, p. 17. */
  int nparam = Int_val(numargs_v);  // dimension of x
  int nineqn = Int_val(nconstr_v);  // number of nonlinear inequality constraints
  double xmin[nparam];         // lower bounds
  double xmax[nparam];         // upper bounds
  double x[nparam];            // initial guess
 
  // fill x xmin xmax from inputs, converting caml values to c values
  int i = 0;
  for(i=0;i<nparam;i++){
    xmin[i] = Double_field(xmin_v,i);
    xmax[i] = Double_field(xmax_v,i);
    x[i] = Double_field(init_v,i);
  }

  int nineq = nineqn;            // total number of ineq constrs.  All constraints are nonlinear, so this is equal to nineqn
  int inform = 0;                // (out) return code
  int nf = 1;                    // number of obj functions
  int mode = 100;                // C=1, B=0, A=0. see manual, p.18
  int iprint = 0;                // how much status output (verbosity, 0 = low)
  int miter = 500;               // max number of iterations
  double bigbnd=1.e10;           // a big number
  double eps=1.0e-8;             // a small number
  double udelta=0.0;             // p.21, should be set to 0.0 if user has no idea how to choose it
  double epsneq = 0.0;           // XXX I have no idea what this does

  int nfsr = 0;                  // we don't use sequentially related functions
  int ncsrl = 0;                 // "
  int ncsrn = 0;                 // "
  int mesh_pts[1] = {0};         // "
  int neqn = 0;                  // we don't use equality constraints
  int neq = 0;                   // "
  void *cd = NULL;               // not used, it's for tricky info passing between functions during optimization

  // Arrays for cfsqp to work
  double *f=(double *)calloc(nf,sizeof(double));
  double *g=(double *)calloc(nineq+neq,sizeof(double));
  double *lambda=(double *)calloc(nineq+neq+nf+nparam,sizeof(double));

  // wish we had a type system...
  cfsqp(nparam,
        nf,
        nfsr,
        nineqn,
        nineq,
        neqn,
        neq,
        ncsrl,
        ncsrn,
        mesh_pts,
        mode,
        iprint,
        miter,
        &inform,
        bigbnd,
        eps,
        epsneq,
        udelta,
        xmin,                   // lower bounds
        xmax,                   // upper bounds
        x,                      // initial guess
        f,
        g,
        lambda,
        caml_obj_fun,           // our objective fun
        caml_constr_fun,        // our constrint funs
        grobfd,                 // from cfsqpusr.h
        grcnfd,                 // from cfsqpusr.h
        cd);                    // dummy
  
  // free storage
  free(f);
  free(g);
  free(lambda);

  double r[1] = {0.0};             // caml_obj_fun puts the value of the obj fun at the optimal point in r

  caml_obj_fun(nparam,0,x,r,cd);

  // make a pair to return the optimal point and the value at that point
  opt_v = caml_alloc_tuple(2);
  Store_field(opt_v, 0, caml_copy_double(r[0]));
  Store_field(opt_v, 1, double_array_to_value(x,nparam));

  CAMLreturn (opt_v);
}


